#ifndef RGBDFrame_H_
#define RGBDFrame_H_

//OpenCV
#include "cv.h"
#include "highgui.h"

//Other
#include <stdio.h>
#include <string.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/features/integral_image_normal.h>
#include <Eigen/Core>
#include "KeyPointSet.h"
#include "Plane.h"
#include "Point.h"
#include "SurfExtractor.h"

using namespace std;
class RGBDCamera;

class RGBDFrame
{

	public:
	string rgb_path;
	string depth_path;
	int id_number;
	double timing;
	
	float focal_length_x;
	float focal_length_y;
	float optical_center_x;
	float optical_center_y;
	int width;
	int height;
	
	FeatureDescriptor * image_descriptor;
	KeyPointSet * keypoints;
	KeyPointSet ** cluster_keypoints;
	int nr_clusters;
	vector<Plane *> planes;
	
	RGBDFrame(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr input_cloud);
	RGBDFrame(RGBDCamera * camera, int number);
	RGBDFrame();
	~RGBDFrame();
/*
	void store(string path);
	void load(string path, RGBDCamera * used_camera);
*/
	void print();
	void showFrame();

	void extractPoints(int &ipa,int &ipb,int &ipc,pcl::PointXYZRGBNormal * & pa, pcl::PointXYZRGBNormal * & pb, pcl::PointXYZRGBNormal * & pc,pcl::PointXYZRGBNormal ** valid_points,int valid_counter);
	void extractPlanes(Point *** pointMat,Point ** points,int nr_points,float ** point_x, float ** point_y, float ** point_z, IplImage* rgb_img,IplImage* depth_img);
	void extractPlanes(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr input_cloud);
	
	//void setupKeyPointData();
	//void setupImageDescriptor();

	//double compareToFrame(RGBDFrame * frame);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr getCloud();
	//RGBDFrame * clone();

};
#endif
